/**
 * @file
 * @brief Point cloud and image visualizer for Ouster Lidar using OpenGL
 *
 * Contains headers and some inline functions for the main PointViz class
 * as well as supporting classes such as Camera, Image, Cloud, etc.
 */
#pragma once

// clang-format off
// glew must be included first so we prevent clang-format from rearranging
#include <GL/glew.h>
#include <GLFW/glfw3.h>
// clang-format on

#include "ouster/colormaps.h"

#include <array>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <iostream>
#include <memory>
#include <mutex>
#include <sstream>
#include <thread>
#include <unordered_map>
#include <vector>
#include <cmath>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

namespace ouster {
namespace viz {
namespace impl {
// we don't align because the visualizer may be compiled with different
// compilation options as internal C++ code, leading to problems. besides, the
// performance here is not super critical
using mat4d = Eigen::Matrix<double, 4, 4, Eigen::DontAlign>;
using mat4f = Eigen::Matrix<GLfloat, 4, 4, Eigen::DontAlign>;

constexpr int default_window_width = 640;
constexpr int default_window_height = 480;
extern int window_width;
extern int window_height;

inline void error_callback(int error, const char* description) {
    std::cerr << "error " << error << std::endl;
    std::cerr << description << std::endl;
}

/**
 * load and compile GLSL shaders
 *
 * @param vertex_shader_code code of vertex shader
 * @param fragment_shader_code code of fragment shader
 * @return handle to program_id
 */
GLuint load_shaders(const std::string& vertex_shader_code,
                    const std::string& fragment_shader_code);

/**
 * load a texture from an array of GLfloat or equivalent
 * such as float[n][3]
 *
 * @param texture array of at least size width * height * elements_per_texel
 *                where elements per texel is 3 for GL_RGB and 1 for GL_RED
 * @param width   width of texture in texels
 * @param height  height of texture in texels
 * @param texture_id handle generated by glGenTextures
 * @param internal_format internal format, e.g. GL_RGB or GL_RGB32F
 * @param format  format, e.g. GL_RGB or GL_RED
 */
template <class F>
void load_texture(const F& texture, const size_t width, const size_t height,
                  const GLuint texture_id,
                  const GLenum internal_format = GL_RGB,
                  const GLenum format = GL_RGB);

/**
 * The point vertex shader supports transforming the point cloud by an array of
 * transformations.
 *
 * @param xyz            XYZ point before it was multiplied by range.
 *                       Corresponds to the "xyzlut" used by LidarScan.
 *
 * @param range          Range of each point.
 *
 * @param key            Key for colouring each point for aesthetic reasons.
 *
 * @param trans_index    Index of which of the transformations to use for this
 *                       point. Normalized between 0 and 1. (0 being the first
 *                       1 being the last).
 *
 * @param model          Extrinsic calibration of the lidar.
 *
 * @param transformation The w transformations are stored as a w x 4 texture.
 *                       Each column of the texture corresponds one 4 x 4
 *                       transformation matrix, where the four pixels' rgb
 *                       values correspond to four columns (3 rotation 1
 *                       translation)
 *
 * @param proj_view      Camera view matrix controlled by the visualizer.
 */
static const std::string point_vertex_shader_code =
    R"SHADER(
            #version 120

            attribute vec3 xyz;
            attribute vec3 offset;
            attribute float range;
            attribute float key;
            attribute vec4 mask;
            attribute float trans_index;

            uniform sampler2D transformation;
            uniform mat4 model;
            uniform mat4 proj_view;

            varying float vcolor;
            varying vec4 overlay_rgba;
            void main(){
                vec4 local_point = range > 0
                                   ? model * vec4(xyz * range + offset, 1.0)
                                   : vec4(0, 0, 0, 1.0);
                // Here, we get the four columns of the transformation.
                // Since this version of GLSL doesn't have texel fetch,
                // we use texture2D instead. Numbers are chosen to index
                // the middle of each pixel.
                // |     r0     |     r1     |     r2     |     t     |
                // 0   0.125  0.25  0.375   0.5  0.625  0.75  0.875   1
                vec4 r0 = texture2D(transformation, vec2(trans_index, 0.125));
                vec4 r1 = texture2D(transformation, vec2(trans_index, 0.375));
                vec4 r2 = texture2D(transformation, vec2(trans_index, 0.625));
                vec4 t = texture2D(transformation, vec2(trans_index, 0.875));
                mat4 car_pose = mat4(
                    r0.x, r0.y, r0.z, 0,
                    r1.x, r1.y, r1.z, 0,
                    r2.x, r2.y, r2.z, 0,
                     t.x,  t.y,  t.z, 1
                );

                gl_Position = proj_view * car_pose * local_point;
                vcolor = sqrt(key);
                overlay_rgba = mask;
            })SHADER";
static const std::string point_fragment_shader_code =
    R"SHADER(
            #version 120
            varying float vcolor;
            varying vec4 overlay_rgba;
            uniform sampler2D palette;
            void main() {
                gl_FragColor = vec4(texture2D(palette, vec2(vcolor, 1)).xyz * (1.0 - overlay_rgba.w)
                              + overlay_rgba.xyz * overlay_rgba.w, 1);
            })SHADER";
static const std::string ring_vertex_shader_code =
    R"SHADER(
            #version 120
            attribute vec3 ring_xyz;
            uniform float ring_range;
            uniform mat4 proj_view;
            void main(){
                gl_Position = proj_view * vec4(ring_xyz * ring_range, 1.0);
                gl_Position.z = gl_Position.w;
            })SHADER";
static const std::string ring_fragment_shader_code =
    R"SHADER(
            #version 120
            void main() {
                gl_FragColor = vec4(0.15, 0.15, 0.15, 1);
            })SHADER";
static const std::string cuboid_vertex_shader_code =
    R"SHADER(
            #version 120
            attribute vec3 cuboid_xyz;
            uniform vec4 cuboid_rgba;
            uniform mat4 pose;
            uniform mat4 proj_view;
            varying vec4 rgba;
            void main(){
                gl_Position = proj_view * pose * vec4(cuboid_xyz, 1.0);
                rgba = cuboid_rgba;
            })SHADER";
static const std::string cuboid_fragment_shader_code =
    R"SHADER(
            #version 120
            varying vec4 rgba;
            void main() {
                gl_FragColor = rgba;
            })SHADER";
static const std::string image_vertex_shader_code =
    R"SHADER(
            #version 120
            attribute vec2 vertex;
            attribute vec2 vertex_uv;
            varying vec2 uv;
            void main() {
                uv = vertex_uv;
                gl_Position = vec4(vertex, -1, 1);
            })SHADER";
static const std::string image_fragment_shader_code =
    R"SHADER(
            #version 120
            varying vec2 uv;
            uniform sampler2D image;
            uniform sampler2D mask;
            void main() {
                vec4 m = texture2D(mask, uv);
                float a = m.a;
                float r = sqrt(texture2D(image, uv).r) * (1.0 - a);
                gl_FragColor = vec4(vec3(r, r, r) + m.rgb * a, 1.0);
            })SHADER";

/**
 * struct containing handles to variables in GLSL shader program compiled from
 * point_vertex_shader_code and point_fragment_shader_code
 */
struct CloudIds {
    GLuint xyz_id, off_id, range_id, key_id, mask_id, model_id, proj_view_id,
        palette_id, transformation_id, trans_index_id;
    CloudIds() {}

    /**
     * constructor
     * @param point_program_id handle to GLSL shader program compiled from
     * point_vertex_shader_code and point_fragment_shader_code
     */
    explicit CloudIds(GLuint point_program_id)
        : xyz_id(glGetAttribLocation(point_program_id, "xyz")),
          off_id(glGetAttribLocation(point_program_id, "offset")),
          range_id(glGetAttribLocation(point_program_id, "range")),
          key_id(glGetAttribLocation(point_program_id, "key")),
          mask_id(glGetAttribLocation(point_program_id, "mask")),
          model_id(glGetUniformLocation(point_program_id, "model")),
          proj_view_id(glGetUniformLocation(point_program_id, "proj_view")),
          palette_id(glGetUniformLocation(point_program_id, "palette")),
          transformation_id(
              glGetUniformLocation(point_program_id, "transformation")),
          trans_index_id(glGetAttribLocation(point_program_id, "trans_index")) {
    }
};

/**
 * Note that most rotations are done in terms of integral decidegrees
 * (tenths of a degree). This is so that the camera will be able to
 * deterministically return to its original orientation if needed, without
 * accumulating floating point error, but at the same time having fine
 * enough subdivisions to have smooth user experience.
 *
 * For your convenience the user-defined literal _deg is defined so that
 * you can convert from degree literals to decidegree easily.
 */
using decidegree = int;
/**
 * convert integral degrees to decidegrees
 */
constexpr decidegree operator""_deg(unsigned long long int angle) {
    return angle * 10;
}
/**
 * convert and round floating point degrees to decidegrees
 */
constexpr decidegree operator""_deg(long double angle) {
    return static_cast<decidegree>(angle * 10);
}
/**
 * convert decidegrees to radians
 */
template <class T>
double decidegree2radian(T angle) {
    return static_cast<double>(M_PI * angle / 180.0_deg);
}

/**
 * camera class
 *
 * The camera class contains the folowing matrices:
 *
 * * Projection matrix proj, determined by view angle
 * * View matrix, relative to the target matrix, controlled by the user
 * * Offset matrix, controlled by the user, a pure translation
 * * Target matrix, usually close to car pose (identity if not using SLAM viz)
 *
 * The final camera matrix is proj * view * offset_mat * target.
 *
 * The camera class performs computations in double because for the SLAM viz,
 * the camera's position may have moved very far from the origin, so extra
 * precision is needed to prevent floating point error.
 */
class Camera {
    /*
     * ViewParameters holds the parameters relevant to the view matrix such as
     * field of view, dolly (distance from target).
     *
     * Likewise, some parameters like focal length and dolly distance are
     * integral.
     */
    struct ViewParameters {
        decidegree yaw;
        double auto_rotate_yaw;
        decidegree pitch;
        int log_focal_length = 0;  // zero means the focal length the same as
                                   // the diagonal size of the window
        int log_distance = 0;      // zero means 50 m
        static constexpr double log_distance_0 = 50.0;
    };

    ViewParameters vp;

    // Here we store time information. Simulation updates to the camera include:
    // 1) auto rotate
    // 2) smooth camera follow
    //
    // These must be simulated in a way that is decoupled from the framerate,
    // so we use real time. steady_clock is used because it is always smoothly
    // and monotonically increasing with the smallest tick interval.
    using viz_clock = std::chrono::steady_clock;
    using viz_time_point = std::chrono::time_point<viz_clock>;
    using viz_duration = std::chrono::nanoseconds;

    viz_time_point last_updated_time;
    viz_time_point rotation_start_time;
    const viz_duration simulation_period =
        std::chrono::duration_cast<viz_duration>(
            std::chrono::duration<double>(1.0 / 144));  // 144 Hz
    const viz_duration auto_rotate_period =
        std::chrono::duration_cast<viz_duration>(
            std::chrono::duration<double>(60.0));

    bool auto_rotate;
    bool orthographic;
    bool target_initialized;
    const double camera_smoothing = 0.005;

    GLfloat x_offset, y_offset;
    std::array<double, 3> offset_3d;

    mat4d view;
    mat4d proj;
    mat4d offset_mat;
    mat4d current_target;
    mat4d desired_target;

    std::mutex desired_target_mutex;

    void setView();

    void setProj();

    void tick();

    void simulate();

   public:
    Camera();

    void reset();

    void setFov(double diagonal_angle_rad);
    void setTarget(const mat4d& mat);

    void left(decidegree amount = 5_deg);
    void right(decidegree amount = 5_deg);
    void up(decidegree amount = 5_deg);
    void down(decidegree amount = 5_deg);

    void zoomIn(int amount = 5);
    void zoomOut(int amount = 5);

    void dollyIn(int amount = 5);
    void dollyOut(int amount = 5);

    void setOffset(GLfloat x, GLfloat y);

    /**
     * set the 3D offset of the point cloud when one middle clicks and drags/
     * @param x_amount pixel amount of dragging horizontally
     * @param y_amount pixel amount of dragging vertically
     */
    void changeOffset3d(double x_amount, double y_amount);

    void update();

    void setAutoRotateOn();
    void setAutoRotateOff();
    void toggleAutoRotate();
    void setOrthographicOn();
    void setOrthographicOff();
    void toggleOrthographic();
    mat4d proj_view() const;
    mat4d proj_view_target() const;
};
}  // namespace impl

/**
 * class to help set up the visualizer
 */
struct CloudSetup {
    // xyz is a non-owning raw pointer.
    // see: https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rr-ptr
    //
    // It should be compatible with raw data from Eigen objects such as
    // ouster::LidarScan::Points or ouster::Points
    const double* xyz;
    const double* off;
    size_t n;
    size_t w;
    std::array<double, 16> extrinsic;
    size_t accumulation;

    /**
     * Set up the input to the PointViz
     *
     * @param xyz_          Cartesian point clouds in the same format as
     *                      impl::Cloud::xyz, compatible with output of
     *                      make_xyz_lut. Non-owning raw pointer.
     * @param n_            Number of points, e.g. 64 * 2048 = 131072
     * @param w_            Number of poses, e.g. 2048
     * @param extrinsic_    Pointer or iterator to array of 16 elements
     *                      containing extrinsic calibration of sensor,
     *                      column major
     * @param accumulation_ Number of point clouds to show, for the slam
     *                      visualization
     */
    template <class T>
    CloudSetup(const double* xyz_, const double* off_, size_t n_, size_t w_,
               const T* extrinsic_, size_t accumulation_ = 1)
        : xyz(xyz_), off(off_), n(n_), w(w_), accumulation(accumulation_) {
        std::copy(extrinsic_, extrinsic_ + 16, extrinsic.begin());
    }
};

namespace impl {

/**
 * Class to deal with point clouds.
 * Each point cloud consists of n points with w poses.
 * The ith point will be transformed by the (i % w)th pose.
 *
 * For example for 2048 x 64 Ouster lidar point cloud,
 * we may have w = 2048 poses and n = 2048 * 64 = 131072.
 *
 * We also keep track of the map pose and the extrinsic calibration as mentioned
 * in the comment in the point_vertex_shader.
 *
 * The map_pose is used to efficiently transform the whole point cloud without
 * having to update all ~2048 poses.
 */
class Cloud {
    const size_t n;
    const size_t w;
    struct CloudBuffers {
        GLuint xyz_buffer;
        GLuint off_buffer;
        GLuint range_buffer;
        GLuint key_buffer;
        GLuint mask_buffer;
        GLuint trans_index_buffer;
    };
    CloudBuffers buffers;
    std::vector<GLfloat> range_data;
    std::vector<GLfloat> key_data;
    std::vector<GLfloat> mask_data;
    std::vector<GLfloat> xyz_data;
    std::vector<GLfloat> off_data;
    std::vector<GLfloat> transformation;  // set automatically by setColumnPoses
    mat4d map_pose;
    std::array<GLfloat, 16> extrinsic_data;  // row major
    GLuint transformation_texture_id;
    bool texture_changed;
    bool mask_changed;
    bool xyz_changed;

   public:
    /**
     * Set up the Cloud. Most of these arguments should correspond to CloudSetup
     *
     * @param xyz        Cartesian point clouds in the same format as
     *                   impl::Cloud::xyz, compatible with output of
     *                   make_xyz_lut.
     * @param off        Cartesian point clouds in the same format as
     *                   impl::Cloud::xyz, compatible with output of
     *                   make_xyz_lut.
     * @param n          Number of points, e.g. 64 * 2048 = 131072
     * @param w          Number of poses, e.g. 2048
     * @param extrinsic  Extrinsic calibration of sensor, col major
     */
    template <class T>
    Cloud(T* xyz, T* off, const size_t n, const size_t w,
          const std::array<double, 16>& extrinsic)
        : n(n),
          w(w),
          range_data(n),
          key_data(n),
          mask_data(4 * n, 0),
          xyz_data(3 * n),
          off_data(3 * n),
          transformation(12 * w, 0),
          texture_changed(true),
          mask_changed(true),
          xyz_changed(true) {
        std::array<GLuint, 6> vertexbuffers;
        glGenBuffers(6, vertexbuffers.data());
        buffers =
            CloudBuffers{vertexbuffers[0], vertexbuffers[1], vertexbuffers[2],
                         vertexbuffers[3], vertexbuffers[4], vertexbuffers[5]};

        map_pose.setIdentity();
        std::vector<GLfloat> trans_index_buffer_data(n);
        for (size_t i = 0; i < n; i++) {
            trans_index_buffer_data[i] = ((i % w) + 0.5) / (GLfloat)w;
        }

        for (size_t v = 0; v < w; v++) {
            transformation[3 * v] = 1;
            transformation[3 * (v + w) + 1] = 1;
            transformation[3 * (v + 2 * w) + 2] = 1;
        }

        glBindBuffer(GL_ARRAY_BUFFER, buffers.trans_index_buffer);
        glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * n,
                     trans_index_buffer_data.data(), GL_STATIC_DRAW);

        glGenTextures(1, &transformation_texture_id);

        setXYZ(xyz);
        setOffset(off);
        std::copy(extrinsic.begin(), extrinsic.end(), extrinsic_data.begin());
    }
    /**
     * set the range values
     *
     * @param x pointer to array of at least as many elements as there are
     *          points, representing the range of the points
     */
    template <class T>
    void setRange(T* x) {
        std::copy(x, x + n, range_data.begin());
    }

    /**
     * set the key values, used for colouring.
     *
     * @param x        pointer to array of at least as many elements as there
     *                 are points, preferably normalized between 0 and 1
     */
    template <class T>
    void setKey(T* x) {
        std::copy(x, x + n, key_data.begin());
    }

    /**
     * set the RGBA mask values, used as an overlay on top of the key
     *
     * @param x        pointer to array of at least 4x as many elements as there
     *                 are points, preferably normalized between 0 and 1
     */
    template <class T>
    void setMask(T* x) {
        std::copy(x, x + 4 * n, mask_data.begin());
        mask_changed = true;
    }

    /**
     * set the XYZ values
     *
     * @param x        pointer to array of exactly 3n where n is number of
     *                 points, so that the xyz position of the ith point is
     *                 i, i + n, i + 2n
     */
    template <class T>
    void setXYZ(T* xyz) {
        for (size_t i = 0; i < n; i++) {
            for (size_t k = 0; k < 3; k++) {
                xyz_data[3 * i + k] = static_cast<GLfloat>(xyz[i + n * k]);
            }
        }
        xyz_changed = true;
    }

    /**
     * set the offset values
     *
     * @param x        pointer to array of exactly 3n where n is number of
     *                 points, so that the xyz position of the ith point is
     *                 i, i + n, i + 2n
     */
    template <class T>
    void setOffset(T* off) {
        for (size_t i = 0; i < n; i++) {
            for (size_t k = 0; k < 3; k++) {
                off_data[3 * i + k] = static_cast<GLfloat>(off[i + n * k]);
            }
        }
    }

    /**
     * set the ith point cloud map pose
     *
     * @param map_pose homogeneous transformation matrix of the pose
     */
    void setMapPose(const mat4d& mat);

    /**
     * Set the per-column poses, so that the point corresponding to the pixel
     * at row u, column v in the staggered lidar scan is transformed by the vth
     * pose, given as a homogeneous transformation matrix.
     *
     * @param rotation    array of rotation matrices, total size 9 * w, where
     *                    the vth rotation matrix is:
     *                    r[v],         r[w + v],     r[2 * w + v],
     *                    r[3 * w + v], r[4 * w + v], r[5 * w + v],
     *                    r[6 * w + v], r[7 * w + v], r[8 * w + v]
     * @param translation translation vector array, column major, where each row
     *                    is a translation vector. That is, the vth translation
     *                    is t[v], t[w + v], t[2 * w + v]
     */
    template <class T>
    void setColumnPoses(T* rotation, T* translation) {
        for (size_t v = 0; v < w; v++) {
            for (size_t u = 0; u < 3; u++) {
                for (size_t rgb = 0; rgb < 3; rgb++) {
                    transformation[(u * w + v) * 3 + rgb] =
                        static_cast<GLfloat>(rotation[v + u * w + 3 * rgb * w]);
                }
            }
            for (size_t rgb = 0; rgb < 3; rgb++) {
                transformation[9 * w + 3 * v + rgb] =
                    static_cast<GLfloat>(translation[v + rgb * w]);
            }
        }
        texture_changed = true;
    }

    /**
     * render the point cloud with the point of view of the Camera
     *
     * @param camera   camera to view the point cloud
     * @param ids      handles to shader program attributes
     * @param palette_texture_id handle to colour map for visualization
     */
    void draw(const Camera& camera, const CloudIds& ids,
              const GLuint palette_texture_id);

    ~Cloud() {
        // todo(dllu) reference count and delete when the last copy of
        // Cloud is deleted glDeleteBuffers(1, &buffers.xyz_buffer);
    }
};

/**
 * Class to maintain a double buffer, so that you can keep writing data
 * without affecting what is being drawn.
 */
template <class T>
class DoubleBuffer {
    std::mutex buffer_mutex;

   public:
    std::unique_ptr<T> write;
    std::unique_ptr<T> read;
    std::atomic_bool enabled;

    /**
     * Constructor
     *
     * @param args arguments for the T::T() constructor
     */
    template <typename... Args>
    explicit DoubleBuffer(Args&&... args)
        : write(new T(std::forward<Args>(args)...)),
          read(new T(*write)),
          enabled(true) {}

    /**
     * calls draw on the readable element in the double buffer
     *
     * @param args arguments for T::draw
     */
    template <typename... Args>
    void draw(Args&&... args) {
        std::lock_guard<std::mutex> guard(buffer_mutex);
        read->draw(std::forward<Args>(args)...);
    }

    /**
     * swaps the readable and writable part of the double buffer, i.e.
     * after all relevant data have been written
     */
    void swap() {
        std::lock_guard<std::mutex> guard(buffer_mutex);
        std::swap(read, write);
    }
};

/**
 * Class that wraps around Cloud to store several double-buffered clouds.
 * This is used for visualizing accumulated point clouds for SLAM visualization
 *
 * Essentially this is a circular buffer of double buffered clouds.
 */
struct MultiCloud {
    std::vector<std::unique_ptr<DoubleBuffer<Cloud>>> clouds;
    size_t index = 0;  // which is the latest cloud to be written to
    bool enabled = true;

    /**
     * Constructor
     * generates a vector of several clouds and initializes them with the same
     * parameters
     *
     * @param setup the CloudSetup object with necessary parameters to
     *              initialize clouds
     */
    explicit MultiCloud(const CloudSetup& setup) : clouds(setup.accumulation) {
        std::generate(clouds.begin(), clouds.end(), [&setup]() {
            return std::unique_ptr<DoubleBuffer<Cloud>>(new DoubleBuffer<Cloud>(
                setup.xyz, setup.off, setup.n, setup.w, setup.extrinsic));
        });
    }

    /**
     * gets the latest writable cloud
     * @return unique ptr to writable cloud
     */
    std::unique_ptr<Cloud>& write() { return clouds[index]->write; }

    /**
     * calls draw on each of the clouds
     * @param Args arguments for Cloud::draw(...)
     */
    template <typename... Args>
    void draw(Args&&... args) {
        for (auto& c : clouds) {
            c->read->draw(std::forward<Args>(args)...);
        }
    }

    /**
     * swaps the double buffer for the latest cloud.
     */
    void swap() {
        clouds[index]->swap();
        index = (index + 1) % clouds.size();
    }
};

/**
 * Class to deal with showing an image. The image is auto-scaled to be
 * positioned either at the top (for wide images) or the left (for tall images)
 */
class Image {
    size_t width;
    size_t height;
    GLfloat aspect_ratio;  // height divided by width of true aspect ratio
    int size_fraction;
    const int size_fraction_max;
    std::vector<GLfloat> data;
    std::vector<GLfloat> mask_data;
    bool texture_changed;
    bool mask_changed;
    std::array<GLuint, 2> vertexbuffers;
    GLuint image_program_id = 0;
    GLuint image_texture_id;
    GLuint mask_texture_id;

   public:
    /**
     * Constructor: create an Image for displaying
     *
     * @param width             pixel width of image
     * @param height            pixel height of image
     * @param size_fraction     fraction of screen to take up, numerator
     * @param size_fraction_max fraction of screen to take up, denominator
     */
    Image(size_t width, size_t height, int size_fraction = 3,
          int size_fraction_max = 10)
        : width(width),
          height(height),
          size_fraction(size_fraction),
          size_fraction_max(size_fraction_max),
          data(width * height, 0),
          mask_data(4 * width * height, 0),
          texture_changed(true),
          mask_changed(true) {}

    /**
     * initialize the image. must be called after valid OpenGL context created
     */
    void initialize() {
        image_program_id =
            load_shaders(image_vertex_shader_code, image_fragment_shader_code);
        glGenBuffers(2, vertexbuffers.data());
        GLuint textures[2];
        glGenTextures(2, textures);
        image_texture_id = textures[0];
        mask_texture_id = textures[1];
    }

    /**
     * set the image
     *
     * @param image_data pointer to array of at least as many elements as there
     *                   are pixels in the image, in row-major format
     */
    template <class T>
    void setImage(T* image_data) {
        const size_t n = width * height;
        std::copy(image_data, image_data + n, data.begin());
        texture_changed = true;
    }

    /**
     * set the RGBA mask
     *
     * @param image_data pointer to array of at least 4x as many elements as
     *                   there are pixels in the image, in row-major format
     */
    template <class T>
    void setMask(T* msk_data) {
        const size_t n = width * height * 4;
        std::copy(msk_data, msk_data + n, mask_data.begin());
        mask_changed = true;
    }

    /**
     * render the monochrome image.
     *
     * @param cam modifies the camera to offset it so that it is centered on the
     *            region not covered by image.
     */
    void draw(Camera& cam);

    /**
     * set pixel dimensions of the image
     *
     * @param w pixel height of image
     * @param h pixel width of image
     */
    void resize(size_t w, size_t h) {
        if (w == width && h == height) return;
        width = w;
        height = h;
        data.resize(w * h);
        mask_data.resize(w * h * 4);
    }

    /**
     * increase or decrease size fraction. Sets member variable
     * size_fraction to be between 0 and size_fraction_max (inclusive)
     *
     * @param amount amount to increase by (can be negative)
     */
    void changeSizeFraction(int amount) {
        size_fraction = (size_fraction + amount + (size_fraction_max + 1)) %
                        (size_fraction_max + 1);
    }

    /**
     * set the display aspect ratio of the image
     *
     * @param a the aspect ratio, i.e. height divided by width
     */
    void setAspectRatio(GLfloat a) { aspect_ratio = a; }

    /**
     * destructor, delete program
     */
    ~Image() { glDeleteProgram(image_program_id); }
};

/**
 * Class to deal with showing a set of rings on the ground as markers to help
 * visualize lidar range.
 */
class Rings {
    const size_t points_per_ring;
    std::vector<GLfloat> xyz;

    GLuint ring_program_id;
    GLuint ring_xyz_id;
    GLuint ring_proj_view_id;
    GLuint ring_range_id;
    GLuint xyz_buffer;

   public:
    int ring_size;
    bool enabled;

    /**
     * constructor
     *
     * @param points_per_ring_ number of points per ring, the more the rounder
     */
    Rings(const size_t points_per_ring_ = 512)
        : points_per_ring(points_per_ring_),
          xyz(points_per_ring * 3, 0),
          ring_size(1),
          enabled(true) {
        for (size_t i = 0; i < points_per_ring; i++) {
            const GLfloat theta = i * 2.0 * M_PI / points_per_ring;
            xyz[3 * i] = std::sin(theta);
            xyz[3 * i + 1] = std::cos(theta);
            xyz[3 * i + 2] = 0.0;
        }
    }

    /**
     * initializes shader program, vertex buffers and handles after OpenGL
     * context has been created
     */
    void initialize() {
        ring_program_id =
            load_shaders(ring_vertex_shader_code, ring_fragment_shader_code);
        ring_xyz_id = glGetAttribLocation(ring_program_id, "ring_xyz");
        ring_proj_view_id = glGetUniformLocation(ring_program_id, "proj_view");
        ring_range_id = glGetUniformLocation(ring_program_id, "ring_range");

        glGenBuffers(1, &xyz_buffer);
        glBindBuffer(GL_ARRAY_BUFFER, xyz_buffer);
        glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * points_per_ring * 3,
                     xyz.data(), GL_STATIC_DRAW);
    }

    /**
     * draws the rings from the point of view of the camera.
     * The rings are always centered on the camera's target.
     *
     * @param camera The camera
     */
    void draw(const Camera& camera) {
        glUseProgram(ring_program_id);
        const float radius = std::pow(10.0f, static_cast<GLfloat>(ring_size));
        mat4f camera_data = camera.proj_view().cast<GLfloat>();
        glUniformMatrix4fv(ring_proj_view_id, 1, GL_FALSE, camera_data.data());
        glEnableVertexAttribArray(ring_xyz_id);
        glBindBuffer(GL_ARRAY_BUFFER, xyz_buffer);
        glVertexAttribPointer(ring_xyz_id,
                              3,         // size
                              GL_FLOAT,  // type
                              GL_FALSE,  // normalized?
                              0,         // stride
                              (void*)0   // array buffer offset
        );
        const GLfloat max_radius = 1000;
        const GLfloat max_rings = 2000;  // for performance
        for (GLfloat r = radius, rr = 0; r < max_radius && rr < max_rings;
             r += radius, rr += 1) {
            glUniform1f(ring_range_id, r);
            glDrawArrays(GL_LINE_LOOP, 0, points_per_ring);
        }
        glDisableVertexAttribArray(ring_xyz_id);
    };

    /**
     * destructor
     */
    ~Rings() { glDeleteProgram(ring_program_id); }
};

/**
 * struct to deal with a single cuboid
 */
struct Cuboid {
    mat4f pose;
    std::array<GLfloat, 4> rgba;
};

/**
 * Class to deal with showing cuboids
 */
class Cuboids {
    const std::array<GLfloat, 24> xyz;
    const std::array<GLubyte, 24> indices;
    const std::array<GLubyte, 24> edge_indices;
    std::vector<Cuboid> cuboids;

    GLuint cuboid_program_id;
    GLuint cuboid_xyz_id;
    GLuint cuboid_proj_view_id;
    GLuint cuboid_pose_id;
    GLuint cuboid_rgba_id;
    GLuint xyz_buffer;

   public:
    bool enabled;

    /**
     * constructor
     */
    Cuboids()
        : xyz{+0.5, +0.5, +0.5, +0.5, +0.5, -0.5, +0.5, -0.5,
              +0.5, +0.5, -0.5, -0.5, -0.5, +0.5, +0.5, -0.5,
              +0.5, -0.5, -0.5, -0.5, +0.5, -0.5, -0.5, -0.5},
          indices{0, 1, 3, 2, 6, 7, 5, 4, 2, 3, 7, 6,
                  4, 5, 1, 0, 0, 2, 6, 4, 5, 7, 3, 1},
          edge_indices{0, 1, 1, 3, 3, 2, 2, 0, 4, 5, 5, 7,
                       7, 6, 6, 4, 0, 4, 1, 5, 2, 6, 3, 7},
          enabled{true} {}

    /**
     * initializes shader program, vertex buffers and handles after OpenGL
     * context has been created
     */
    void initialize() {
        cuboid_program_id = load_shaders(cuboid_vertex_shader_code,
                                         cuboid_fragment_shader_code);
        cuboid_xyz_id = glGetAttribLocation(cuboid_program_id, "cuboid_xyz");
        cuboid_proj_view_id =
            glGetUniformLocation(cuboid_program_id, "proj_view");
        cuboid_pose_id = glGetUniformLocation(cuboid_program_id, "pose");
        cuboid_rgba_id = glGetUniformLocation(cuboid_program_id, "cuboid_rgba");

        glGenBuffers(1, &xyz_buffer);
        glBindBuffer(GL_ARRAY_BUFFER, xyz_buffer);
        glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * 24, xyz.data(),
                     GL_STATIC_DRAW);
    }

    void clear() { cuboids.clear(); }
    void push(Cuboid&& c) { cuboids.push_back(std::move(c)); }

    /**
     * draws the cuboids from the point of view of the camera.
     * The cuboids are always centered on the camera's target.
     *
     * @param camera The camera
     */
    void draw(const Camera& camera) {
        glUseProgram(cuboid_program_id);
        const mat4f camera_data = camera.proj_view_target().cast<GLfloat>();
        glUniformMatrix4fv(cuboid_proj_view_id, 1, GL_FALSE,
                           camera_data.data());
        glEnableVertexAttribArray(cuboid_xyz_id);
        glBindBuffer(GL_ARRAY_BUFFER, xyz_buffer);
        glVertexAttribPointer(cuboid_xyz_id,
                              3,         // size
                              GL_FLOAT,  // type
                              GL_FALSE,  // normalized?
                              0,         // stride
                              (void*)0   // array buffer offset
        );
        for (const auto& cuboid : cuboids) {
            const mat4f pose = cuboid.pose;
            glUniformMatrix4fv(cuboid_pose_id, 1, GL_FALSE, pose.data());
            glUniform4fv(cuboid_rgba_id, 1, cuboid.rgba.data());

            glDrawElements(GL_QUADS, 24, GL_UNSIGNED_BYTE, indices.data());
            auto rgba = cuboid.rgba;
            rgba[3] = 1;
            glUniform4fv(cuboid_rgba_id, 1, rgba.data());
            glDrawElements(GL_LINES, 24, GL_UNSIGNED_BYTE, edge_indices.data());
        }
        glDisableVertexAttribArray(cuboid_xyz_id);
    };

    /**
     * destructor
     */
    ~Cuboids() { glDeleteProgram(cuboid_program_id); }
};
}  // namespace impl

class PointViz {
    static std::unordered_map<GLFWwindow*, PointViz*> window_to_viz;
    std::vector<CloudSetup> viz_setups;

    std::vector<impl::MultiCloud> clouds;
    impl::DoubleBuffer<impl::Image> image;
    impl::DoubleBuffer<impl::Cuboids> cuboids;
    std::string name;
    GLFWwindow* window;
    impl::Camera camera;
    impl::Rings rings;

    GLuint palette_texture_id;
    GLfloat point_program_id;
    GLfloat point_size;
    bool lbutton_down;
    bool mbutton_down;
    double mouse_x;
    double mouse_y;

    impl::CloudIds cloud_ids;

    std::unordered_multimap<int, std::function<void()>> key_handlers_;
    std::thread window_thread;
    std::mutex init_mutex;
    bool initialized;
    std::condition_variable init_condition;

   public:
    std::atomic_bool quit;
    using idx = std::ptrdiff_t;

    /**
     * constructor
     *
     * @param viz_setups_ for setting up point clouds, typically one per sensor
     * @param name_       name of the visualizer, shown in the title bar
     * @param fork_       whether or not to run the draw/poll loop in a separate
     *                    thread.
     *                    PLEASE NOTE: the fork will not work on macOS because
     *                    mac only supports running graphical stuff on the main
     *                    thread.
     */
    PointViz(std::vector<CloudSetup>&& viz_setups_, const std::string& name_,
             const bool fork = true)
        : viz_setups{std::move(viz_setups_)},
          image(0, 0),
          name(name_),
          point_size(3),
          lbutton_down(false),
          mbutton_down(false),
          mouse_x(0),
          mouse_y(0),
          initialized(false),
          quit(false) {
        image.enabled = false;
        if (fork) {
            window_thread = std::thread([this]() {
                bool success = false;
                {
                    std::unique_lock<std::mutex> init_lock(init_mutex);
                    success = this->initialize();
                }
                this->initialized = true;
                this->init_condition.notify_one();
                if (!success) return;
                this->drawLoop();
            });
            std::unique_lock<std::mutex> init_lock(init_mutex);
            init_condition.wait(init_lock,
                                [this]() { return this->initialized; });
        } else {
            // initialize on the main thread
            initialize();
        }
    }

    /**
     * main drawing loop, keeps drawing things while quit is false
     */
    void drawLoop();

    /**
     * set the ith point cloud with range values
     *
     * @param cloud_id index of which cloud to update
     * @param x pointer to array of at least as many elements as there are
     *          points, representing the range of the points
     */
    template <class T>
    void setRange(const idx cloud_id, T* x) {
        auto& c = clouds[cloud_id].write();
        c->setRange(x);
    }

    /**
     * set the ith point cloud with key values, used for colouring.
     *
     * @param cloud_id index of which cloud to update
     * @param x        pointer to array of at least as many elements as there
     *                 are points, preferably normalized between 0 and 1
     */
    template <class T>
    void setKey(const idx cloud_id, T* x) {
        auto& c = clouds[cloud_id].write();
        c->setKey(x);
    }

    /**
     * convenience function equivalent to setRange and setKey
     *
     * @param cloud_id index of which cloud to update
     * @param r        range
     * @param k        key
     */
    template <class T, class U>
    void setRangeAndKey(const idx cloud_id, T* r, U* k) {
        setRange(cloud_id, r);
        setKey(cloud_id, k);
    }

    /**
     * set the ith point cloud with new XYZ values
     *
     * @param cloud_id index of which cloud to update
     * @param x        pointer to array of exactly 3n where n is number of
     *                 points, so that the xyz position of the ith point is
     *                 i, i + n, i + 2n
     */
    template <class T>
    void setXYZ(const idx cloud_id, T* x) {
        auto& c = clouds[cloud_id].write();
        c->setXYZ(x);
    }

    /**
     * set the ith point cloud with new offset values
     *
     * @param cloud_id index of which cloud to update
     * @param x        pointer to array of exactly 3n where n is number of
     *                 points, so that the xyz position of the ith point is
     *                 i, i + n, i + 2n
     */
    template <class T>
    void setOffset(const idx cloud_id, T* x) {
        auto& c = clouds[cloud_id].write();
        c->setOffset(x);
    }

    /**
     * set the ith point cloud map pose
     *
     * @param cloud_id index of which cloud to update
     * @param map_pose homogeneous transformation matrix of the pose
     */
    void setMapPose(const idx cloud_id, const impl::mat4d& map_pose) {
        auto& c = clouds[cloud_id].write();
        c->setMapPose(map_pose);
    }

    /**
     * Set the poses for the ith point cloud
     *
     * @param cloud_id index of which cloud to update
     * @param rotation rotation matrix 9 by w column major
     * @param translation translation vector array column major
     */
    template <class T>
    void setColumnPoses(const idx cloud_id, T* rotation, T* translation) {
        auto& c = clouds[cloud_id].write();
        c->setColumnPoses(rotation, translation);
    }

    /**
     * set the mask for the cloud
     *
     * @param mask pointer to array of at least 4x as many elements as there
     *             are points, in rgba format
     */
    template <class T>
    void setCloudMask(const idx cloud_id, const T* msk_data) {
        auto& c = clouds[cloud_id].write();
        c->setMask(msk_data);
    }

    /**
     * Swap double buffered cloud after writing all necessary data
     *
     * @param i index of which cloud to swap
     */
    void cloudSwap(size_t i) { clouds[i].swap(); }

    /**
     * set the image
     *
     * @param image_data pointer to array of at least as many elements as there
     *                   are pixels in the image, in row-major format
     */
    template <class T>
    void setImage(const T* image_data) {
        image.enabled = true;
        image.write->setImage(image_data);
    }

    /**
     * set the mask for the image
     *
     * @param mask pointer to array of at least 4x as many elements as there
     *             are pixels, in rgba format
     */
    template <class T>
    void setImageMask(const T* msk_data) {
        image.write->setMask(msk_data);
    }

    /**
     * set the pixel dimensions of the image. disables the image if either is 0
     *
     * @param w pixel height of image
     * @param h pixel width of image
     */
    void resizeImage(size_t w, size_t h) {
        if (w == 0 || h == 0) {
            image.enabled = false;
        } else {
            image.enabled = true;
            image.write->resize(w, h);
        }
    }

    /**
     * set the display aspect ratio of the image
     *
     * @param a the aspect ratio, i.e. height divided by width
     */
    void setImageAspectRatio(GLfloat a) { image.write->setAspectRatio(a); }

    /**
     * disable showing the image
     */
    void disableImage() { image.enabled = false; }

    /**
     * swap the double buffered image after writing all necessary data
     */
    void imageSwap() { image.swap(); }

    /**
     * add a cuboid
     *
     * @param a cuboid
     */
    void addCuboid(impl::Cuboid&& cuboid) {
        cuboids.write->push(std::move(cuboid));
    }

    /**
     * swap double buffered cuboids
     */
    void cuboidSwap() {
        cuboids.read->clear();
        cuboids.swap();
    }

    /**
     * set camera target. the camera will smoothly move towards it
     */
    void setCameraTarget(const impl::mat4d& target) {
        camera.setTarget(target);
    }

    /**
     * add custom key handler to the key. Multiple bindings can exist
     * for the same key.
     *
     * @param key which key to bind to, e.g. GLFW_KEY_A
     * @param f   function to execute after key is pressed
     */
    void attachKeyHandler(int key, std::function<void()>&& f) {
        key_handlers_.insert(std::make_pair<int, std::function<void()>>(
            std::move(key), std::move(f)));
    }

    /*
     * execute custom key handlers for the key
     *
     * @param key which key to press, e.g. GLFW_KEY_A
     */
    void callKeyHandlers(const int key) {
        auto p = key_handlers_.equal_range(key);
        for (auto& it = p.first; it != p.second; ++it) {
            it->second();
        }
    }

    /**
     * destructor, joins the window thread
     */
    ~PointViz() {
        quit = true;
        if (window_thread.joinable()) {
            window_thread.join();
        }
    }

   private:
    /**
     * sets up the visualizer, creating OpenGL context, compiling shaders,
     * allocating buffers, etc.
     */
    bool initialize();

    /**
     * callback for resizing the window. Should be called automatically by GLFW.
     *
     * @param window pointer to GLFW window
     * @param width  window width in pixels
     * @param height window height in pixels
     */
    static void updateWindowSize(GLFWwindow* window, int width, int height) {
        impl::window_width = width;
        impl::window_height = height;
        window_to_viz[window]->camera.update();
        glViewport(0, 0, width, height);
    }

    /**
     * callback for keypress. Polled after each drawing.
     * Called automatically by GLFW.
     *
     * @param window pointer to GLFW window
     * @param key    which key was pressed, e.g. GLFW_KEY_A
     * @param action whether key was released or pressed, e.g. GLFW_PRESS
     */
    static void handleKeyPress(GLFWwindow* window, int key, int /*scancode*/,
                               int action, int mods) {
        if (action != GLFW_PRESS && action != GLFW_REPEAT) {
            return;
        }
        auto pthis = window_to_viz[window];
        if (mods == 0) {
            if (key == GLFW_KEY_R) {
                pthis->camera.toggleAutoRotate();
            }
            if (key == GLFW_KEY_W) {
                pthis->camera.up();
            }
            if (key == GLFW_KEY_S) {
                pthis->camera.down();
            }
            if (key == GLFW_KEY_A) {
                pthis->camera.left();
            }
            if (key == GLFW_KEY_D) {
                pthis->camera.right();
            }
            if (key == GLFW_KEY_E) {
                pthis->image.write->changeSizeFraction(1);
                pthis->image.read->changeSizeFraction(1);
            }
            if (key == GLFW_KEY_O) {
                pthis->point_size =
                    std::max(static_cast<GLfloat>(1), pthis->point_size - 1);
                glPointSize(pthis->point_size);
                std::cerr << "point size decreased: " << pthis->point_size
                          << std::endl;
            }
            if (key == GLFW_KEY_P) {
                pthis->point_size =
                    std::min(static_cast<GLfloat>(10), pthis->point_size + 1);
                glPointSize(pthis->point_size);
                std::cerr << "point size increased: " << pthis->point_size
                          << std::endl;
            }
            if (key == GLFW_KEY_SEMICOLON) {
                pthis->rings.ring_size =
                    std::min(2, pthis->rings.ring_size + 1);
                std::cerr << "ring size increased: 10^"
                          << pthis->rings.ring_size << std::endl;
            }
            if (key == GLFW_KEY_APOSTROPHE) {
                pthis->rings.ring_size =
                    std::max(-2, pthis->rings.ring_size - 1);
                std::cerr << "ring size decreased: 10^"
                          << pthis->rings.ring_size << std::endl;
            }
            if (key == GLFW_KEY_EQUAL) {
                pthis->camera.zoomIn();
            }
            if (key == GLFW_KEY_MINUS) {
                pthis->camera.zoomOut();
            }
            if (key >= GLFW_KEY_1 && key <= GLFW_KEY_9) {
                size_t cloud_id = static_cast<size_t>(key - GLFW_KEY_1);
                if (cloud_id < pthis->clouds.size()) {
                    pthis->clouds[cloud_id].enabled =
                        !pthis->clouds[cloud_id].enabled;
                }
                for (size_t i = 0; i < pthis->clouds.size(); i++) {
                    std::cerr << i + 1;
                }
                std::cerr << std::endl;
                for (const auto& cloud : pthis->clouds) {
                    if (cloud.enabled) {
                        std::cerr << "*";
                    } else {
                        std::cerr << " ";
                    }
                }
                std::cerr << std::endl;
            }
            if (key == GLFW_KEY_0) {
                pthis->camera.toggleOrthographic();
            }
        } else if (mods == GLFW_MOD_SHIFT) {
            if (key == GLFW_KEY_R) {
                // reset camera
                pthis->camera.reset();
            }
        }

        // process custom key handlers from users
        pthis->callKeyHandlers(key);
    }

    /**
     * callback for mouse press. Polled after each drawing.
     * Keeps track of whether mouse is held down with lbutton_down member
     * variable. Called automatically by GLFW.
     *
     * shift + left click is the same as middle click to support people with
     * very few mouse buttons
     *
     * @param window pointer to GLFW window
     * @param button which button was pressed, e.g. GLFW_MOUSE_BUTTON_LEFT
     * @param action whether key was released or pressed, e.g. GLFW_PRESS
     */
    static void handleMouseButton(GLFWwindow* window, int button, int action,
                                  int mods) {
        auto pthis = window_to_viz[window];
        if (GLFW_PRESS == action) {
            if (button == GLFW_MOUSE_BUTTON_LEFT && mods == 0) {
                pthis->lbutton_down = true;
            } else if (button == GLFW_MOUSE_BUTTON_MIDDLE ||
                       (button == GLFW_MOUSE_BUTTON_LEFT &&
                        mods == GLFW_MOD_SHIFT)) {
                pthis->mbutton_down = true;
            }
        }

        if (GLFW_RELEASE == action) {
            pthis->lbutton_down = false;
            pthis->mbutton_down = false;
        }
    }

    /**
     * callback for cursor movement.
     * If mouse is held down, this is used for dragging, and updates camera.
     * Called automatically by GLFW.
     *
     * @param window pointer to GLFW window
     * @param xpos   subpixel x position of mouse
     * @param ypos   subpixel y position of mouse
     */
    static void handleCursorPos(GLFWwindow* window, double xpos, double ypos) {
        auto pthis = window_to_viz[window];
        const double dx = (xpos - pthis->mouse_x);
        const double dy = (ypos - pthis->mouse_y);
        if (pthis->lbutton_down) {
            constexpr double sensitivity = 3;
            if (dx > 0) {
                pthis->camera.left(sensitivity * dx);
            } else {
                pthis->camera.right(-sensitivity * dx);
            }
            if (dy > 0) {
                pthis->camera.up(sensitivity * dy);
            } else {
                pthis->camera.down(-sensitivity * dy);
            }
        } else if (pthis->mbutton_down) {
            pthis->camera.changeOffset3d(dx, dy);
        }
        pthis->mouse_x = xpos;
        pthis->mouse_y = ypos;
    }

    /**
     * callback for mouse scroll
     * Used for dollying the camera.
     * Called automatically by GLFW.
     *
     * @param window pointer to GLFW window
     * @param xoff   horizontal scroll amount (unused)
     * @param yoff   vertical scroll amount
     */
    static void handleScroll(GLFWwindow* window, double xoff, double yoff) {
        (void)xoff;
        auto pthis = window_to_viz[window];
        if (yoff > 0) {
            pthis->camera.dollyIn(yoff * 5);
        } else {
            pthis->camera.dollyOut(-yoff * 5);
        }
    }

    /**
     * callback for mouse entering or leaving the window
     * Used to avoid the "sticky" situation where one clicks and drags but
     * releases the mouse button outside of the window, and then the next time
     * when the mouse re-enters the window, it still thinks the mouse is pressed
     * causing the camera to freak out when the user moves the mouse around.
     * Called automatically by GLFW.
     *
     * @param window  pointer to GLFW window
     * @param entered whether it entered or left the window
     */
    static void handleCursorEnter(GLFWwindow* window, int entered) {
        auto pthis = window_to_viz[window];
        if (entered) {
            // do something? idk
        } else {
            pthis->lbutton_down = false;
            pthis->mbutton_down = false;
        }
    }
};

/**
 * load a texture from an array of GLfloat or equivalent
 * such as float[n][3]
 *
 * @param texture array of at least size width * height * elements_per_texel
 *                where elements per texel is 3 for GL_RGB and 1 for GL_RED
 * @param width   width of texture in texels
 * @param height  height of texture in texels
 * @param texture_id handle generated by glGenTextures
 * @param internal_format internal format, e.g. GL_RGB or GL_RGB32F
 * @param format  format, e.g. GL_RGB or GL_RED
 */
template <class F>
void impl::load_texture(const F& texture, const size_t width,
                        const size_t height, const GLuint texture_id,
                        const GLenum internal_format, const GLenum format) {
    glBindTexture(GL_TEXTURE_2D, texture_id);

    // we have only 1 level, so we override base/max levels
    // https://www.khronos.org/opengl/wiki/Common_Mistakes#Creating_a_complete_texture
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAX_LEVEL, 0);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_BASE_LEVEL, 0);

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

    glTexImage2D(GL_TEXTURE_2D, 0, internal_format, width, height, 0, format,
                 GL_FLOAT, texture);
}

}  // namespace viz
}  // namespace ouster
